from periphery import PWM, GPIO


class Motor(PWM):
    def __init__(self, 
        pwmchip, pwm_channel,
        ctrl0, ctrl1
    ):
        super().__init__(pwmchip, pwm_channel)

        self.ctrl0 = GPIO(*ctrl0)
        self.ctrl1 = GPIO(*ctrl1)
    
    def set_dir(self, dir):
        if dir == 0 or dir == "forward":
            self.ctrl0.write(False)
            self.ctrl1.write(True)
        
        elif dir == 1 or dir == "backward":
            self.ctrl0.write(True)
            self.ctrl1.write(False)

    def set_throttle(self, thr, rate = 0.2):
        
        if thr >= 0:
            self.set_dir(0)
            # self.duty_cycle = rate * thr
            self.duty_cycle = thr
        else:
            self.set_dir(1)
            # self.duty_cycle = rate * (- thr)
            self.duty_cycle = -thr


    def close(self):
        self.ctrl0.close()
        self.ctrl1.close()
        self.disable()
        
        super().close()


if __name__ == "__main__":
    import time, os

    os.system("sh get_privilege.sh")

    laser = GPIO("/dev/gpiochip1", 22, "out")
    laser.write(False)
    time.sleep(0.5)
    laser.write(True)
    time.sleep(0.5)
    laser.write(False)

    motor_left = Motor(4, 0, ("/dev/gpiochip0", 29, "out"), ("/dev/gpiochip4", 11, "out"))
    motor_right = Motor(0, 0, ("/dev/gpiochip1", 18, "out"), ("/dev/gpiochip1", 20, "out"))

    motor_left.set_dir("forward")
    motor_right.set_dir("forward")

    motor_left.frequency = 500
    motor_right.frequency = 500

    motor_left.polarity = "normal"
    motor_right.polarity = "normal"

    motor_left.duty_cycle = 0
    motor_right.duty_cycle = 0

    motor_left.enable()
    motor_right.enable()

    # for i in range(0, 40):
    #     motor_left.duty_cycle = i / 100
    #     motor_right.duty_cycle = i / 100

    #     print(motor_right.duty_cycle, motor_right.duty_cycle)

    #     time.sleep(0.1)
    
    # motor_left.set_dir("backward")
    # motor_right.set_dir("backward")

    # for i in range(0, 30, 10):
    #     motor_left.duty_cycle = i / 100
    #     motor_right.duty_cycle = i / 100

    #     print(motor_right.duty_cycle, motor_right.duty_cycle)

    #     time.sleep(1)


    # motor_left.set_dir("backward")
    # motor_right.set_dir("forward")

    # motor_left.duty_cycle = 20 / 100 * 0.90
    # motor_right.duty_cycle = 20 / 100

    # time.sleep(30)

    # motor_left.close()
    # motor_right.close()
    # laser.close()
